{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# PYNQ Car using DFROBOT Turtle: 2WD Mobile Robot Platform\n",
    "\n",
    "<img src=\"./images/pynq_car.jpg\" style=\"width:400px;\" />\n",
    "\n",
    "---"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Aim\n",
    "\n",
    "* This notebook illustrates how to use available APIs for the Geared Motor and DC Motor Driver on PYNQ-Z2 PMOD interface and build the PYNQ Car using DFROBOT Turtle: 2WD Mobile Robot Platform.\n",
    "\n",
    "## Peripherals\n",
    "| Module                                                       | \tQuantity |\n",
    "| ------------------------------------------------------------ | ------ |\n",
    "| [Turtle: 2WD Mobile Robot Platform for Arduino](https://www.dfrobot.com/product-65.html) | 1      |\n",
    "| [Gravity: Smart Grayscale Sensor](https://www.dfrobot.com/product-1007.html) | 2      |\n",
    "| [Gravity: URM09 Ultrasonic Sensor (Trig)](https://www.dfrobot.com/product-2172.html) | 1      |\n",
    "| [7.4V Lipo 2500mAh Battery (Arduino Power Jack)](https://www.dfrobot.com/product-489.html) | 1      |\n",
    "| [Gravity: 2x1.2A DC Motor Driver with Gravity Connector (TB6612FNG)](https://www.dfrobot.com/product-1705.html) | 1      |\n",
    "| [Grove - 4 pin Male Jumper to Grove 4 pin Conversion Cable (5 PCs per Pack)](https://www.seeedstudio.com/Grove-4-pin-Male-Jumper-to-Grove-4-pin-Conversion-Cable-5-PCs-per-Pack.html)| 1    |\n",
    "| [Breadboard Jumper Cables High Quality (30 Pack)](https://www.dfrobot.com/product-516.html)|1|\n",
    "| [Gravity: TT Motor Encoders Kit](https://www.dfrobot.com/product-98.html) (Optional) | 1 |\n",
    "| [9g Metal Gear Micro Servo (1.8Kg )](https://www.dfrobot.com/product-1338.html)(Optional) | 1 |\n",
    "\n",
    "## Last revised\n",
    "* 21 Oct 2021\n",
    "    + DFRobot sensor version\n",
    "---"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1.Load _base_ Overlay"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pynq.overlays.base import BaseOverlay\n",
    "from pynq_peripherals import ArduinoDIGILENTGroveAdapter, PmodGroveAdapter\n",
    "base = BaseOverlay('base.bit')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<!-- ## Control the motor with PYNQ PMOD -->\n",
    "## 2. Make Physical Connections"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-box alert-warning\"><ul>\n",
    "    <h4 class=\"alert-heading\">Make Physical Connections of Motors</h4>\n",
    "    <li>Connect the motors to motor driver and the motor driver should be powered by a DC source larger than 6V. </li><li>Connect the green wires from motor driver PWM1 and PWM2 to PMODA pin 2 and PMODB pin 2 respectively, connect the green wire of DIR1 and DIR2 to PMODA pin 6 and PMODB pin 6 repectively. </li><li> The black wires from motor driver should connect to PMOD ground pins.</li><li> The red wires from motor driver are left floating, hence they can be unplugged or cut from the connector.</li>\n",
    "</ul>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"./images/motor_system.png\"  style=\"width:450px;\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-box alert-warning\"><ul>\n",
    "    <h4 class=\"alert-heading\">Make Physical Connections of Sensors</h4>\n",
    "    <li>Insert the Digilent Grove Base Shield into the Arduino connector on the board. Connect two Smart Grayscale Sensors to G5 and G6 connector of the Digilent Grove Base Shield respectively.</li>\n",
    "    <li>Connect the Usranger module to G4 connector of the Grove Base Shield</li>\n",
    "    <li>The two line finders should be aligned together in front of the turtle.</li>\n",
    "</ul>\n",
    "</div>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"images/sensor_connection.png\" style=\"width:450px;\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-box alert-info\"><ul>\n",
    "    <h4 class=\"alert-heading\">Notes: Turn off the motor power now to avoid unexpected motion during configuration</h4>\n",
    "</ul>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 3. Configure adapter and peripherals"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Adapter configuration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "adapter_A = PmodGroveAdapter(base.PMODA, G4='geared_motor')\n",
    "adapter_B = PmodGroveAdapter(base.PMODB, G4='geared_motor')\n",
    "arduino_adapter = ArduinoDIGILENTGroveAdapter(base.ARDUINO, G6='grove_line_finder',G5='grove_line_finder',G4='grove_usranger')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Define motor object"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-box alert-info\"><ul>\n",
    "    <h4 class=\"alert-heading\">Notes: </h4>\n",
    "    <li><b>Set the direction:</b>  Use  forward() and  backforward() function to go forward or backward respectively.</li>\n",
    "    <li><b>Set the speed:</b> Valid values are from -100 to 100. The car will go forward if the speed is positive and go backword if the speed is negative.  </li>\n",
    "</ul>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "right_motor = adapter_A.G4\n",
    "left_motor = adapter_B.G4\n",
    "right_motor.backward()\n",
    "left_motor.backward()\n",
    "right_motor.set_speed(0)\n",
    "left_motor.set_speed(0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Define sensor object"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "left_line_finder = arduino_adapter.G5\n",
    "right_line_finder = arduino_adapter.G6\n",
    "usranger = arduino_adapter.G4"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 4. Test peripherals"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Calibrate smart grayscale sensors\n",
    "Just short click the button and the mode will be changed between analog and digital.\n",
    "\n",
    "Here, we use the digital mode (detailed instruction could be found in the [specification](https://wiki.dfrobot.com/Smart_Grayscale_sensor_SKU_SEN0147) of smart grayscale sensor)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-box alert-warning\"><ul>\n",
    "    <h4 class=\"alert-heading\">Set the logic of two different color by your own</h4>\n",
    "    <li> Long click the button until the LED slow blinking (which means first color detecting ready). Let the grayscale sensor face to the first color (such as white paper). Meanwhile short click the button and the LED gets dark (which means first color detected).</li>\n",
    "    <li> Short click the button and make the LED fast blink (which means second color detecting ready). Let the grayscale sensor face to the second color (such as black paper). Meanwhile short click the button and the LED gets dark (which means second color detected).</li>\n",
    "    <li>The first color (such as white paper) will be Logical '1', and the LED becomes bright. The second color (such as black paper) will be Logical '0', and the LED becomes dark. This setting maintains even if the power is off, so for one purpose, one setting is enough.</li>\n",
    "    </ul>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"images/BlackLight.png\" style=\"width:350px;\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Test sensors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "- Put your hand in front of the Usranger to test the get_distance() function\n",
    "- Smart Grayscale Sensor shall output various logics for black and white surface"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "distance: 500.00 cm\n",
      "True\n",
      "True\n"
     ]
    }
   ],
   "source": [
    "print('distance: {:.2f} cm'.format(usranger.get_distance()))\n",
    "print(left_line_finder.line_found())\n",
    "print(right_line_finder.line_found())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Test motors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "- The car should rotate for 3 seconds"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-box alert-info\"><ul>\n",
    "    <h4 class=\"alert-heading\">Turn on the motor power now.</h4>\n",
    "</ul>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "def rotate(speed):\n",
    "    right_motor.forward()\n",
    "    left_motor.backward()\n",
    "    right_motor.set_speed(speed)\n",
    "    left_motor.set_speed(speed)\n",
    "    \n",
    "def stop():\n",
    "    left_motor.set_speed(0)\n",
    "    right_motor.set_speed(0)\n",
    "    left_motor.backward()\n",
    "    right_motor.backward()\n",
    "    \n",
    "import time\n",
    "\n",
    "rotate(40)\n",
    "time.sleep(3)\n",
    "stop()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. PYNQ Car Competition"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following figure shows a sample map. Your car should be programmed to run from the **start line** to the **end line**.\n",
    "\n",
    "Notes:\n",
    "- Two smart grayscalse sensors shall be used to track black lines and adjust direction.\n",
    "- One usranger shall be used to detect obstacle and help steering."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-box alert-info\"><ul>\n",
    "    <h4 class=\"alert-heading\">Make a map and have a try!</h4>\n",
    "</ul>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"images/map.png\" style=\"width:450px;\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Copyright (C) 2021 Xilinx, Inc\n",
    "\n",
    "SPDX-License-Identifier: BSD-3-Clause"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "----"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "----"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
